#include <zephyr/kernel.h>
#include <zephyr/shell/shell.h>
#include <version.h>
#include <zephyr/logging/log.h>
#include <stdlib.h>
#include <zephyr/drivers/uart.h>
#include <zephyr/usb/usb_device.h>
#include <ctype.h>
#include <zephyr/arch/cpu.h>
#include <zephyr/sys/util.h>
#include <zephyr/linker/linker-defs.h>
#include <zephyr/device.h>
#include <zephyr/shell/shell_uart.h>
#include <zephyr/drivers/flash.h>
#include <zephyr/irq.h>
#include <zephyr/drivers/watchdog.h>
#include <zephyr/devicetree.h>
#include "include/motor.h"
#include "include/relay.h"
#include "include/jetson_nano.h"
LOG_MODULE_REGISTER(nano);

void shell_reinit_trigger(void);

static uint8_t rx_buf[NANO_FRAME_LEN];
static int rx_buf_pos = 0;
static bool rx_flag;
extern Motor_Ctr 	Motor_Ctr_g;
extern Motor_Para 	Motor_Para_g;
extern Stepmotor_Sta Stepmotor_Sta_g;
extern Servo_Sta Servo_Sta_g;
extern Sensor_Sta Sensor_Sta_g;
extern k_tid_t nano_tx_tid;

extern const struct device *uart_motor;
const struct device *uart_shell = DEVICE_DT_GET(DT_CHOSEN(zephyr_shell_uart));
#define NANO_CTR_MSGQ_MAX 10
/* queue to store up to 10 messages (aligned to 4-byte boundary) */
K_MSGQ_DEFINE(nano_uart_rx_msgq, NANO_FRAME_LEN, NANO_CTR_MSGQ_MAX, 4);

void nano_uart_callback(const struct device *dev, void *user_data)
{
	uint8_t c;
    static uint8_t nano_tail;

	uart_irq_update(dev);

	while (uart_irq_rx_ready(dev)) {
		uart_fifo_read(dev, &c, 1);
        if(nano_tail == NANO_FRAME_HEAD_LOW && c == NANO_FRAME_HEAD_HIGH ){
            if(rx_buf_pos != 0){
                rx_buf_pos = 0;
            }
            rx_flag = true;
            rx_buf[rx_buf_pos++] = NANO_FRAME_HEAD_LOW;
        }
            
        if(rx_flag){
            rx_buf[rx_buf_pos++] = c;
        }
        if(rx_buf_pos == NANO_FRAME_LEN){
            k_msgq_put(&nano_uart_rx_msgq, &rx_buf, K_NO_WAIT);
            rx_buf_pos = 0;
            rx_flag = false;
        }
        nano_tail = c;
	}
}

void nano_rx_dispense(void *p1, void *p2, void *p3)
{
    uint8_t last_spd_data[6];
    while(1){
        Host_Ctr_Cmd ctr_cmd_frame = {0};
        if(k_msgq_get(&nano_uart_rx_msgq,&ctr_cmd_frame, K_FOREVER) == 0){
            uint16_t crc16 = CRC16_ModBus((uint8_t *)&ctr_cmd_frame, NANO_FRAME_LEN_NOCRC);
            if(ctr_cmd_frame.crc16L != (uint8_t)crc16 ||  ctr_cmd_frame.crc16H != (uint8_t)(crc16 >> 8)){
                continue;
            }

            if(ctr_cmd_frame.cmd == SHELL_OPEN){
				shell_reinit_trigger();
                k_thread_suspend(nano_tx_tid);
            }

            if(ctr_cmd_frame.cmd == POWER_ON){
                Relay_open(ALL);
            }
            if(ctr_cmd_frame.cmd == POWER_OFF){
                Relay_close(ALL);
            }
            if(ctr_cmd_frame.cmd == JOYSTICK_CMD){
                for(int i = 0; i < 6; ++i){
                    if(ctr_cmd_frame.motor_ctr.data[i] != last_spd_data[i]){
                        for(int j = 0; j < 6; ++j){
                            last_spd_data[j] = ctr_cmd_frame.motor_ctr.data[j];
                        }
                        Motor_Ctr_API(uart_motor,ctr_cmd_frame.motor_ctr);
                        break;
                    }
                }
                Stepmotor_ctr_API(ctr_cmd_frame.stepmotor_ctr);
                Servo_ctr_API(ctr_cmd_frame.servo_ctr);
            }
        }
        k_sleep(K_MSEC(10));
    }
}

void nano_tx_dispense(void *p1, void *p2, void *p3)
{
    k_thread_suspend(nano_tx_tid); //默认关闭该线程
    while(1){
         Dev_Sta_Data dev_sta_data = {
            .motor_sta = Motor_Para_g,
            .stepmotor_sta = Stepmotor_Sta_g,
            .servo_sta = Servo_Sta_g,
            .sensor_sta = Sensor_Sta_g
        };
        dev_sta_data.headL = NANO_FRAME_HEAD_LOW;
        dev_sta_data.headH = NANO_FRAME_HEAD_HIGH;
        dev_sta_data.cmd = DEV_STA_DATA;
        uint16_t crc16 = CRC16_ModBus((uint8_t *)&dev_sta_data, DEV_FRAME_LEN_NOCRC);
        dev_sta_data.crc16L = (unsigned char)crc16;
        dev_sta_data.crc16H =  (unsigned char)(crc16 >> 8);
        uint8_t *data = (uint8_t *)&dev_sta_data;
        //LOG_INF("motor temperature: %d °c" "  motor speed: "LOG_FLOAT_MARKER "dps""  motor volt: " LOG_FLOAT_MARKER "V""  motor curr: " LOG_FLOAT_MARKER "A""  motor degree: " LOG_FLOAT_MARKER "°"" \r\n",dev_sta_data.motor_sta.motor_tem, LOG_FLOAT(dev_sta_data.motor_sta.motor_spd), LOG_FLOAT(dev_sta_data.motor_sta.motor_vol),LOG_FLOAT(dev_sta_data.motor_sta.motor_curr), LOG_FLOAT(dev_sta_data.motor_sta.motor_degree));
        for(int i = 0; i < DEV_FRAME_LEN; ++i){
            uart_poll_out(uart_shell, *(data + i));
        }
        k_sleep(K_MSEC(10));
    }
}